package def;

// PID class to be written by you
public class PID {

  // Private attribute containing a reference to the GUI
	private PIDGUI GUI;
	
  // Private attribute containing a reference to the PIDParameters
  // currently used
	private PIDParameters p;
	private double I;
	private double v;
	private double e;
	private double y, yOld;
	private double D;
	private double ad, bd;


  // Additional attributes


  // Constructor
  public PID(String name) {
	  PIDParameters p = new PIDParameters();
	  p.Beta = 1.0;
	  p.integratorOn = false;
	  p.Ti = 0.0;
	  p.Tr = 10.0;
	  p.K = -0.1;
	  p.Td = 1.7;
	  p.H = 0.1;
	  p.N = 7;
	  
	  new PIDGUI(this, p, name);
	  setParameters(p);
	  
	 
	  
	  this.I = 0.0;
	  this.v = 0.0;
	  this.e = 0.0;
  }


	public synchronized double calculateOutput(double y, double yref) {
		
		this.y = y;
		e = yref - y;
		D = ad*D - bd*(y - yOld);
		v = p.K*(p.Beta*yref - y) + I + D;
		
		return v;
	}

  // Updates the controller state. Should use tracking-based anti-windup
  // Called from BeamAndBallRegul.
  public synchronized void updateState(double u) {
	  if (p.integratorOn)
		  I = I + (p.K * p.H / p.Ti) * e + (p.H / p.Tr) * (u - v);
	  yOld = y;
  }

  // Returns the sampling interval expressed as a long. Explicit type casting
  // needed
  public synchronized long getHMillis() {
	  return (long)(p.H * 1000.0);
  }

  // Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
	public synchronized void setParameters(PIDParameters newParameters) {
		p = (PIDParameters)newParameters.clone();
		if (!p.integratorOn) {
			I = 0.0;
		}
		ad = p.Td / (p.Td + p.N * p.H);
		bd = p.K * ad * p.N;
	}

}